/*
This file is part of MMM.

MMM is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.

MMM is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU General Public License for more details.

You should have received a copy of the GNU General Public License
along with MMM.  If not, see <http://www.gnu.org/licenses/>.
*
* @package    MMM
* @author     Andre Meixner
* @copyright  2017 High Performance Humanoid Technologies (H2T), Karlsruhe, Germany
*
*/

#ifndef __MMM_Motion_H_
#define __MMM_Motion_H_

#include "../MMMCore.h"
#include "../MMMImportExport.h"
#include "../Model/Model.h"
#include "../Model/ModelProcessor.h"
#include "Sensor.h"
#include "SensorMeasurement.h"

#include <string> 
#include <vector>
#include <set>
#include <map>

namespace MMM
{
class Motion;

typedef boost::shared_ptr<Motion> MotionPtr;
typedef std::vector<MotionPtr> MotionList;

/*! @brief Stores a recorded motion with a corresponding virtual model and real measurements */
class MMM_IMPORT_EXPORT Motion
{
public:

    Motion(const std::string &name, ModelPtr orginalModel = nullptr, ModelPtr processedModel = nullptr, ModelProcessorPtr modelProcessor = nullptr, const std::string &originFilePath = std::string());

    /*! Returns a clone of this motion */
    MotionPtr clone();

    void setName(const std::string& name);

    std::string getName();

    /*! Returns origin path when loaded from file. */
    std::string getOriginFilePath();

    /*! Return the model.
        @param processed Indicates weather the processed or the original model should be returned (If no modelProcessor is specified, the originalModel is always returned).
        @return The model or ModelPtr() if no model is not specified. */
    ModelPtr getModel(bool processed = true);
    
    /*! Return the ModelProcessor (if specified). */
    ModelProcessorPtr getModelProcessor();

    //! get a segmented motion with name 'MOTIONNAME_segmented_STARTTIMESTEPf-ENDTIMESTEPf' according to timesteps.
    MotionPtr getSegmentMotion(float startTimestep, float endTimestep, bool changeTimestep = false);

    //! get a segmented motion with the given name according to timesteps.
    MotionPtr getSegmentMotion(float startTimestep, float endTimestep, std::string segmentMotionName, bool changeTimestep = false);

    //! Returns the sensor with the given name in the sensor data map.
    SensorPtr getSensorByName(const std::string &name);

    //! Returns the first sensor with the given sensor type (or nullptr) and cast it to a specific subclass sensor.
    template <typename S>
    boost::shared_ptr<S> getSensorByType(const std::string &type) {
        return boost::dynamic_pointer_cast<S>(getSensorByType(type));
    }

    //! Returns the first sensor with the given sensor type or nullptr.
    SensorPtr getSensorByType(const std::string &type);

    //! Returns all sensor with the given sensor type and cast them to the specific subclass sensor. Removes nullptr after cast.
    template <typename S>
    std::vector<boost::shared_ptr<S> > getSensorsByType(const std::string &type) {
        std::vector<boost::shared_ptr<S> > sensors;
        for (auto sensor : getSensorsByType(type)) {
            boost::shared_ptr<S> cast_sensor = boost::dynamic_pointer_cast<S>(sensor);
            if (cast_sensor) sensors.push_back(cast_sensor);
        }
        return sensors;
    }

    //! Returns all sensor with the given sensor type.
    SensorList getSensorsByType(const std::string &type);

    //! Returns true if the motion contains at least one sensor with the given type.
    bool hasSensor(const std::string &type);

    //! Returns true if the motion contains any sensor.
    bool hasSensor();

    //! Returns a map of the motion's sensor names pointing to their sensor measurement at the given timestep (if present).
    std::map<std::string, SensorMeasurementPtr> getAllMeasurementsForTimeStep(float timestep);

    //! Returns the sensor data as map.
    std::map<std::string, SensorPtr> getSensorData();

    //! Get Sensors sorted after their xml priority (descending order).
    std::vector<SensorPtr> getPrioritySortedSensorData();

    /*! Adds a new sensor to the motion. If present the name of the sensor in the motion matches the unique name of the sensor, else the sensor type is set as name.
        Ascending numbers starting with 2 are added if the motion already contains a sensor matching the sensor type.
        @param sensor The sensor.
        @param delta Timeshift of the sensor compared to the sensors of the motion.
        @throws MMMException if a sensor with the same configuration is already contained or the sensor not matching the model. */
    void addSensor(SensorPtr sensor, float delta = 0.0f);

    //! Returns a sensor in the motion having the same configuration.
    SensorPtr getSensor(SensorPtr sensor);

    //! Joins two motions by joining their sensors and returning a corresponding joined Motion.
    static MotionPtr join(MotionPtr motion1, MotionPtr motion2, std::string name = std::string());

    //! Returns the minimum timestep of all sensors' measurements. If no timesteps are found, returns 0.0f.
    float getMinTimestep();

    //! Returns the maximum timestep of all sensors' measurements. If no timesteps are found, returns 0.0f.
    float getMaxTimestep();

    //! Returns the common minimum timestep of all sensors' measurements. If no timesteps are found, returns 0.0f.
    float getCommonMinTimestep(const std::set<std::string> &ignoreSensorNames = std::set<std::string>());

    //! Returns the common maximum timestep of all sensors' measurements. If no timesteps are found, returns 0.0f.
    float getCommonMaxTimestep(const std::set<std::string> &ignoreSensorNames = std::set<std::string>());

    //! Synchronizes all possible sensormeasurements on a specific time frequency via linear interpolation. Removes all sensors, that are not linear interpolatable.
    void synchronizeSensorMeasurements(float timeFrequency);

    //! Returns if the motion is synchronized
    bool isSynchronized();

    //! Extend kinematic and model pose sensors containing only a single measurement
    void extendKinematic();

    //! Returns all sensor types contained the motion
    std::set<std::string> getSensorTypes();

    //! Static method to replace a motion with the same name in a list of motions with the given motion.
    static MotionList replaceMotion(MotionPtr motion, MotionList motions);

    //! Static method to replace a motion with the same name in a list of motions with the given motion. Otherwise the motion will just be added.
    static MotionList replaceAddMotion(MotionPtr motion, MotionList motions);

    /*! Returns a motion with the specific name from a list or if motionName is empty the first motion from the list
        @param motions The list of motions
        @param motionName The specific name of the motion
        @throws XMLFormatException When not possible */
    static MotionPtr getMotion(MotionList motions, const std::string &motionName = std::string());

    static std::tuple<float, float> calculateMinMaxTimesteps(MotionList motions);

    static void calculateMinMaxTimesteps(MMM::MotionList motions, float &minTimestep, float &maxTimestep);

protected:

    bool addSensor(std::string name, SensorPtr sensor, float delta);

	std::string name;
    std::string originFilePath;
    ModelPtr originalModel;
    ModelPtr processedModel; // the processed model
    ModelProcessorPtr modelProcessor;

    std::map<std::string, SensorPtr> sensorData;
};

}

#endif
